/*  Fish schooling code - based on the OpenCL Nvidia simpleGL example and Andy Johnson's colliding galaxies */

#define offset 16.0
#define xMax 2000
#define yMax 2000
#define zMax 2000
#define mass .005
#define desiredSep 4.0

#define fishTriangleSize 12.0f
#define vertPerFish 3 

#define baseFishVelMax 1.0f//.5f
#define baseFishVelMin .1f//.05f

static inline float4 makeTriangleBase( float x , float y, float vx, float vy, float sizeOfTriangle )
{    
    float2 norm = normalize( (float2)(vx, vy) );
    float2 refPt = (float2) (x-norm.x*sizeOfTriangle/3.0, y-norm.y*sizeOfTriangle/3.0);
    float2 pt1 = (float2)( refPt.x-norm.y*sizeOfTriangle/3.0, refPt.y+norm.x*sizeOfTriangle/3.0);
    float2 pt2 = (float2)( refPt.x+norm.y*sizeOfTriangle/3.0, refPt.y-norm.x*sizeOfTriangle/3.0);
    
    return (float4)(pt1.x, pt1.y, pt2.x, pt2.y);
}

static inline float2 makeTrianglePoint( float x , float y, float vx, float vy, float sizeOfTriangle )
{
    float2 norm = normalize( (float2)(vx, vy) );
    float2 pt1 = (float2) (x+norm.x*2.0*sizeOfTriangle/3.0, y+norm.y*2.0*sizeOfTriangle/3.0);   
    return (float2) (pt1.x, pt1.y);
}

//Front point
static inline float4 makeTrianglePoint1( float4 loc, float4 vel, float sizeOfTriangle )
{   
    float4 norm = normalize( vel );//normalize the velocity-- focus on direction
    float3 toReturn = loc.xyz + norm.xyz * 2.0f*sizeOfTriangle/3.0f;
    return (float4)(toReturn.x, toReturn.y, toReturn.z, 1.0 ); 
}

//Back point
static inline float4 makeTrianglePoint2( float4 loc, float4 vel, float sizeOfTriangle )
{   
    float4 norm = normalize( vel );//normalize the velocity-- focus on direction
    float3 refPt = loc.xyz - norm.xyz*sizeOfTriangle/3.0f; //(float2) (x-norm.x*sizeOfTriangle/3.0, y-norm.y*sizeOfTriangle/3.0);
    float2 pt2 = (float2)( refPt.x-norm.y*sizeOfTriangle/3.0f, refPt.y+norm.x*sizeOfTriangle/3.0f);
    return (float4)(pt2.x, pt2.y, refPt.z-3.0, 1.0f ); 
}

//Back point
static inline float4 makeTrianglePoint3( float4 loc, float4 vel, float sizeOfTriangle )
{   
    float4 norm = normalize( vel );//normalize the velocity-- focus on direction
    float3 refPt = loc.xyz - norm.xyz*sizeOfTriangle/3.0f; //(float2) (x-norm.x*sizeOfTriangle/3.0, y-norm.y*sizeOfTriangle/3.0);
    float2 pt3 = (float2)( refPt.x+norm.y*sizeOfTriangle/3.0f, refPt.y-norm.x*sizeOfTriangle/3.0f);
    return (float4)(pt3.x, pt3.y, refPt.z-3.0, 1.0f ); 
}


static inline float4 stepwiseIntersect( float4 p1, float4 v, int pix )
{
    float denom = sqrt( v.x*v.x + v.y*v.y );
    if( denom == 0.0 )
        return (float4)(-1000.0, -1000.0, -1000.0, -1000.0);
    float pixPerTimeStep = 10;
    float timeStep = pixPerTimeStep/denom;//.5 / denom;
    float stop = (float)pix/pixPerTimeStep;
    for( int i = 1; i <= (int)stop; i++)
    {
        float4 coords = p1 + v*i*timeStep;
        if( coords.x <= 0 || coords.y <= 0 || coords.z <= 0 || coords.x >= xMax || coords.y >= yMax || coords.z >= zMax )//test if in bounds
        {
            return coords;
        }
    }
    return (float4)(-1000.0, -1000.0, -1000.0, -1000.0);    
}


////determines if there is an intersection with an obstacle
////if so, determines how soon the intersection will occur
////computes the force proportional to the vel and the distance , in the direction lateral to the vel (eg. 90deg from vel) 
////braking assumed to have already been applied
static inline float4 computeObstacleSteeringForce( float4 loc, float4 vel, float theta )
{
    int d = 50;
    float4 redirect = (float4)(0.0, 0.0, 0.0, 0.0);
    
    float4 ray = (float4) ( (vel.x*cos( theta ) - vel.y * sin(theta)) , (vel.x*sin(theta) + vel.y*cos(theta) ), vel.z, 1.0 ); //ray in the directon of theta- in plane with z
    
    float4 intPt = stepwiseIntersect( loc, ray, d );//find distance to intersection with obstacle
    if( intPt.x > -1000.0 )//if intersects
    {
        float dist = distance( intPt.xyzw , loc.xyzw );//distance between boid and obstacle
        if( dist <= (float)d )
        {
            float magV = sqrt( vel.x*vel.x+vel.y*vel.y+vel.z*vel.z);
            float vHatX = -vel.y / magV;
            float vHatY = vel.x / magV; //this gives the lateral unit vector 
            
            redirect.x = ((-2.0*magV) / sqrt(dist)) * sin(theta) * vHatX;
            redirect.y = ((-2.0*magV) / sqrt(dist)) * sin(theta) * vHatY;
            redirect.z = 0.0;
            redirect.w = 0.0;
        }
    }
    return redirect;
}


static inline float4 computeBrakingForce( float4 loc, float4 vel )
{
    int d = 50;
    float4 brake = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 intersectPt = stepwiseIntersect(loc, vel, d ); //returns -1000.0 if no intersection within distance
    if( intersectPt.x > -1000.0 ) //if it intersects
    {
        float dist = distance( intersectPt.xyzw , loc.xyzw );//distance between obstacle and boid
        if( dist <= (float) d )//if close
        {
            float4 dif = intersectPt.xyzw - loc.xyzw;//get vector away from obstacle
            brake -= dif;//and define it as redirect 
            brake = normalize( brake );//normalize
            float magV = sqrt( vel.x*vel.x + vel.y*vel.y +vel.z*vel.z); //magnitude of the velocity
            brake.xyzw = brake.xyzw * magV / sqrt(dist) ; // magnitude of the force away from the obstacle should be inversely proportional to the dist^2 
            //and proportional to the magnitude of the velocity 
        }
        
        return brake;
    }
}


//vertex data: .... x, y, z, w .... r g b a ...
//fish pos data: x y z w
//fish vel data vx vy vz vw 
//fish schooling data: coh sep ali perceptual range


__kernel void soManyFish(__global float4* vertexData, __global float4* fishPosData, __global float4* fishVelData, __global float4* fishSchoolingData, __global float4* fishInformedData,  __global bool* isInformed, unsigned int width, unsigned int height, unsigned int numFish, unsigned int time, float velLevel)
{
    // vertexData contains vec4 positions for numFish - vert buffer- followed by vec4 colours for numFish
    
    unsigned int x = get_global_id(0);//get my global id in both dim
    unsigned int y = get_global_id(1);
    
    //INDICES
    unsigned int idx = y*width+x;
    unsigned int vertexDataLoc1   = y*width*vertPerFish+x*vertPerFish;
    if( idx >= numFish )
        return;
    unsigned int vertexDataLoc2 = vertexDataLoc1 + 1;
    unsigned int vertexDataLoc3 = vertexDataLoc1 + 2;
	unsigned int correspondingLoc1 = width*height*vertPerFish + vertexDataLoc1;
    unsigned int correspondingLoc2 = correspondingLoc1 + 1;
    unsigned int correspondingLoc3 = correspondingLoc1 + 2;
    
    //FISH POSITION
    float4 position = fishPosData[ idx ]; 
    float4 velocity = fishVelData[ idx ]; //get velocity for this fish
    float4 schoolingData = fishSchoolingData[idx];
    
    //VERTEX POSITION AND COLOR VARIABLES
    float4 vertPos1 = vertexData[ vertexDataLoc1 ];
    float4 vertPos2 = vertexData[ vertexDataLoc2 ];
    float4 vertPos3 = vertexData[ vertexDataLoc3 ];
    float4 vertColor1 = vertexData[ correspondingLoc1 ];
    float4 vertColor2 = vertexData[ correspondingLoc2 ];
    float4 vertColor3 = vertexData[ correspondingLoc3 ];
    
    //CHANGE IN MOVEMENT
    float4 separation = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 dif = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 cohesion = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 centerOfNeighbors = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 alignment = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 avgVelOfNeighbors = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 avoidWall = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 pursue = (float4)(0.0, 0.0, 0.0, 0.0);
    float4 flee = (float4)(0.0, 0.0, 0.0, 0.0);
    float countNeighbors = 0.0;
    
    float4 informedData = fishInformedData[ idx ];
    float4 locOfTarget = (float4)(informedData.xyz, 1.0 );//will only use if is informed
    bool informed = (locOfTarget.x != -1.0); //isInformed[ idx ];
    bool informed1 = (locOfTarget.x == 100.0);
    bool informed2 = (locOfTarget.x == 1900.0);
    
    if( time < 2 ) //initialize
    {
        vertPos1 = makeTrianglePoint1( position, velocity, fishTriangleSize );
        vertPos2 = makeTrianglePoint2( position, velocity, fishTriangleSize ); //(float4)( 100.0, 0.0, 0.0, 1.0 );
        vertPos3 = makeTrianglePoint3( position, velocity, fishTriangleSize ); //(float4)( 0.0, 100.0, 0.0, 1.0 );
        
        vertColor1 = (float4)(22.0/255.0, 188.0/255.0, 232.0/255.0, 1.0);//color is blue
        vertColor2 = (float4)(142.0/255.0, 228.0/255.0, 232.0/255.0, 1.0);//1.0, 1.0, 0.0, 1.0);//color is yellow
        vertColor3 = (float4)(142.0/255.0, 228.0/255.0, 232.0/255.0, 1.0);//(1.0, 99.0/255.0, 148.0, 1.0);//(1.0, 1.0, 0.0, 1.0);//color is yellow
        
        if( informed1 )
        {
            vertColor1 = (float4)(1.0, 0.0, 0.0, 0.0); 
            vertColor2 = (float4)(1.0, 0.0, 0.0, 0.0); 
            vertColor3 = (float4)(1.0, 0.0, 0.0, 0.0);
        }
        else if( informed2 )
        {
            vertColor1 = (float4)(1.0, 1.0, 0.0, 0.0); 
            vertColor2 = (float4)(1.0, 1.0, 0.0, 0.0); 
            vertColor3 = (float4)(1.0, 1.0, 0.0, 0.0);
        }
    }
    
    //SUM COHESION, ALIGNMENT AND SEPARATION FORCES
    for(int i = 0 ; i < numFish; i++) //later add 
    {
        if( i != idx && i < numFish )
        {
            float4 otherPosition = fishPosData[ i ];
            //evaluate if fish is a 'neighbor' that can influence this fish's behavior
            float d = fabs( distance( otherPosition.xyzw, position.xyzw ));
            if( d < (schoolingData.w * fishTriangleSize) ) //note- later will try to make this be 'if in fish field of view'
            {
                //cohesion- find avg position of neighboring fish
                centerOfNeighbors.xyzw += otherPosition.xyzw;
                
                //alignment
                float4 otherVelocity = fishVelData[ i ];
                avgVelOfNeighbors.xyzw += otherVelocity.xyzw;
                
                countNeighbors = countNeighbors + 1.0;
            }
            if( d < fishTriangleSize*2.0f )
            {
                dif.xyzw = otherPosition.xyzw - position.xyzw;
                separation.xyzw -= dif.xyzw;
                
            }
        }
    }
    
    if( countNeighbors > 0.0 )//avoid divide by zero problem
    {
        centerOfNeighbors.xyzw /= countNeighbors;
        avgVelOfNeighbors /= countNeighbors;
        
        cohesion.xyzw = centerOfNeighbors.xyzw - position.xyzw;
        alignment.xyzw = avgVelOfNeighbors.xyzw - velocity.xyzw; 
    }
    
    //if isInformed, TARGET 
    float4 target = (float4)(0.0, 0.0, 0.0, 0.0);
    if( informed )
    {
        target = locOfTarget - position;
        //        vertColor1 = (float4)( 1.0, 0.0, 0.0, 1.0);
        //        vertColor2 = (float4)( 1.0, 1.0, 0.0, 1.0);
        //        vertColor3 = (float4)( 1.0, 1.0, 0.0, 1.0);
        target = normalize( target );
    }
    else
    {
        //        vertColor1 = (float4)(22.0/255.0, 188.0/255.0, 232.0/255.0, 1.0);//color is blue
        //        vertColor2 = (float4)(142.0/255.0, 228.0/255.0, 232.0/255.0, 1.0);//1.0, 1.0, 0.0, 1.0);//color is yellow
        //        vertColor3 = (float4)(142.0/255.0, 228.0/255.0, 232.0/255.0, 1.0);//(1.0, 99.0/255.0, 148.0, 1.0);//(1.0, 1.0, 0.0, 1.0);//color is yellow  
    }
    
    //APPLY FORCES TO VELOCITY-- make sure it is within velocity limits
    float4 newVelocity = velocity.xyzw + cohesion.xyzw * schoolingData.x + alignment.xyzw * schoolingData.z + target.xyzw*informedData.z;
    if( separation.x != 0.0 && separation.y != 0.0 && separation.z != 0.0 )
    {
        newVelocity.xyzw = velocity.xyzw + separation.xyzw * schoolingData.y;
    }
    
    //MAKE SURE IT STAYS IN BOUND
    float4 redirect = (float4)(0.0, 0.0, 0.0, 0.0);
    redirect += computeBrakingForce( position, normalize( newVelocity )  );
    
    float sinTheta = sqrt(2.0)/2.0;//sin 45
    float theta = (float)(3.1415 / 6.0);
    redirect += computeObstacleSteeringForce( position, normalize( newVelocity ), theta );
    redirect += computeObstacleSteeringForce( position, normalize( newVelocity ), -1.0*theta );
    
    if( redirect.x != 0.0 && redirect.y != 0.0 && redirect.z != 0.0 )
        newVelocity.xyzw = redirect.xyzw;//normalize(redirect.xyzw);
    
    //CONSTRAIN VELOCITY
    float l = fabs( (float) length(newVelocity.xyzw) );
    if( l > (baseFishVelMax * (float) velLevel) )//if greater than max vel (considering current velocity level)
    {
        newVelocity.xyzw = normalize( newVelocity.xyzw ) * baseFishVelMax * (float)velLevel;  
    }
    else if( l < (baseFishVelMin * (float) velLevel) )
    {
        newVelocity.xyzw = normalize( newVelocity.xyzw ) * baseFishVelMin * (float)velLevel;      
    }
    
    //CONSTRAIN CHANGE IN VELOCITY ANGLE
    float angle = acos( (newVelocity.x*velocity.x + newVelocity.y*velocity.y + newVelocity.z*velocity.z) / ( length( newVelocity) * length( velocity ) ) );
    float maxAngle = .314;//can only go +- 18 degrees 
    if( angle > maxAngle )
    {
        float4 w = cross( velocity , newVelocity );
        w = normalize( w );
        float deltaAngle = maxAngle - angle;  //delta angle is the neg diff between max and current angle
        float4 newVelocityDir = newVelocity * cos( deltaAngle ) + cross( w, newVelocity ) * sin( deltaAngle ) + w * dot( w, newVelocity ) * ( 1.0f-cos(deltaAngle) );
        newVelocity = normalize( newVelocityDir ) * length( newVelocity ); 
        
        angle = acos( (newVelocity.x*velocity.x + newVelocity.y*velocity.y + newVelocity.z*velocity.z) / ( length( newVelocity) * length( velocity ) ) );
    }    
    
    //UPDATE POSITION
    position = position.xyzw + newVelocity.xyzw; 
    vertPos1 = makeTrianglePoint1( position, velocity, fishTriangleSize );
    vertPos2 = makeTrianglePoint2( position, velocity, fishTriangleSize ); //(float4)( 100.0, 0.0, 0.0, 1.0 );
    vertPos3 = makeTrianglePoint3( position, velocity, fishTriangleSize ); //(float4)( 0.0, 100.0, 0.0, 1.0 );
    
    //WRITE CHANGES BACK TO GLOBAL MEMORY
    fishPosData[idx] = position;
    fishVelData[idx] = newVelocity;
    vertexData[ vertexDataLoc1 ] = vertPos1;
    vertexData[ vertexDataLoc2 ] = vertPos2; 
    vertexData[ vertexDataLoc3 ] = vertPos3;
    vertexData[ correspondingLoc1 ] = vertColor1;//NOTE- may not need to do this, if not changing color
    vertexData[ correspondingLoc2 ] = vertColor2;
    vertexData[ correspondingLoc3 ] = vertColor3;
    
    
}